`timescale 1ns / 1ps
/*--------------------------------------------------------------------*\
FileName        : cbb_StepMotor.v
Author          ：hpy
Email           ：yuan_hp@qq.com
Date            ：2025年05月09日
Description     ：4线步进电机控制模块

该模块控制速度的过程如下所示，为了节省硬件资源，加速和减速阶段并非均匀的
        _______
加速-->/       \ 
______/         \减速阶段
      |          \_______________________
      |           |     EXT_STEPS   |
    启动速度       减速到最小速度     额外按最小速度走一点

1.例化模板
加减速
cbb_StepMotor #(
    .FCLK(1000_000),    // 系统时钟
    .W(32) ,            // 步进电机位宽
    .SW (32),           // 位移记录的位宽
    .EN_ACC(1'b1),      // 使能加减速
    .MIN_FS( 10_000 ),  // 最小速度
    .MAX_FS (20_000),   // 最大速度
    .INIT_FS ( 10_000 ),// 启动速度
    .ACCEL (8 ),        // 加速度，每次加速减小的分频系数
    .DECEL (2 ),        // 减速度，每次减速增加的分频系数
    .EXT_STEPS( 10) ,   //  减速到最小速度后，按最小速度走的步数,避免速度刚下来有过冲失步
    .ADJUST_STEPS (1)   // 多少个脉冲调节一次速度
) u_cbb_StepMotor (
    .clk( clk ),
    .rst_n( rst_n) ,
    .i_divider(25) ,            // 时钟分频值，脉冲频率 为fclk/2/i_divider，只在 EN_ACC=1 时生效
    .i_step(step) ,             // 要走的位移，正负表示方向 
    .i_start(start) ,           // 上升沿触发一次运动
    .i_zero(zero) ,             // 外部光电开关或其他器件表示的步进电机0位置
    .i_autoStopAtZero(1'b1) ,   // 是否需要自动停止，当找到零点时 ,1:自动停止
    // 反馈信号
    .o_busy() ,                 // 是否忙
    .o_x() ,                    // 当前位移 
    // 驱动器接口
    .o_pulse(),      // 步进电机脉冲输出
    .o_dir(),        // 步进电机方向
    .o_en()          // 步进电机使能
);

无加减速
cbb_StepMotor #(
    .FCLK(1000_000),    // 系统时钟
    .W(32) ,            // 步进电机位宽
    .SW (32),           // 位移记录的位宽
    .EN_ACC(1'b0),      // 使能加减速
) u_cbb_StepMotor (
    .clk( clk ),
    .rst_n( rst_n) ,
    .i_divider(25) ,            // 时钟分频值，脉冲频率 为fclk/2/i_divider，只在 EN_ACC=1 时生效
    .i_step(step) ,             // 要走的位移，正负表示方向 
    .i_start(start) ,           // 上升沿触发一次运动
    .i_zero(zero) ,             // 外部光电开关或其他器件表示的步进电机0位置
    .i_autoStopAtZero(1'b1) ,   // 是否需要自动停止，当找到零点时 ,1:自动停止
    // 反馈信号
    .o_busy() ,                 // 是否忙
    .o_x() ,                    // 当前位移 
    // 驱动器接口
    .o_pulse(),      // 步进电机脉冲输出
    .o_dir(),        // 步进电机方向
    .o_en()          // 步进电机使能
);
\*--------------------------------------------------------------------*/
`ifndef _cbb_StepMotor_

`ifdef DEBUG
  `define debug(debug_command) debug_command
`else
  `define debug(debug_command)
`endif
module cbb_StepMotor #(
    parameter FCLK = 50000000 ,// 系统时钟频率,Hz
    parameter W = 32,          // 步进电机位宽
    parameter SW = 32,         // 位移记录的位宽
    parameter EN_ACC = 0,      // 使能加减速 0:没有减减速  1: 伪的梯形加减速  

    parameter MAX_FS = 20000 , // 驱动步进电机的脉冲最大频率 ,Hz
    parameter MIN_FS = 10000 , // 驱动步进电机的脉冲最大频率 ,Hz
    parameter INIT_FS= MIN_FS ,// 步进电机启动时的脉冲频率，默认为最小频率 ,Hz

    parameter ACCEL = 2,       // 加速度，每次加速减小的分频系数
    parameter DECEL = 2,       // 减速度，每次减速增加的分频系数
    parameter EXT_STEPS = 10,  // 减速到最小速度后，按最小速度走的步数  
    parameter ADJUST_STEPS = 1 // 多少个脉冲调节一次速度
)(
    input clk,
    input rst_n ,
    input wire         [W-1:0] i_divider ,
    input wire signed  [W-1:0] i_step  ,  // 要走的位移，正负表示方向 
    input wire         i_start ,
    input wire         i_zero , //
    input wire         i_autoStopAtZero , // 是否需要自动停止，当找到零点时 
    output wire        o_busy,
    output wire signed [SW-1:0]o_x ,//  有符号数

    output reg         o_pulse , 
    output reg         o_dir   ,
    output reg         o_en    
    
);

/* ------------------ function -------------------- */
function clog2(input integer v);
begin
    for(clog2=0;v>0;clog2=clog2+1)
        v = v>>1;
end
endfunction 


function [W-1:0] SpeedDown(input [W-1:0] cu_divi) ;
begin
    SpeedDown = cu_divi + DECEL ; 
    if(SpeedDown > MIN_SPEED)
        SpeedDown = MIN_SPEED;
    else 
        SpeedDown = SpeedDown ;
end
endfunction 

function [W-1:0] SpeedUp(input [W-1:0] cu_divi) ;
begin
    if(cu_divi >= ACCEL)
        SpeedUp = cu_divi - ACCEL; 
    else 
        SpeedUp = 0 ;

    if(SpeedUp < MAX_SPEED)
        SpeedUp = MAX_SPEED;
    else 
        SpeedUp = SpeedUp ;
end
endfunction 

/* -------------------- param --------------------- */
localparam 
    IDLE = 2'd0,
    SYNC = 2'd1,
    LOW  = 2'd2,
    HIGH = 2'd3;

localparam MAX_SPEED = FCLK / MAX_FS;   // 最大速度，对应最小分频系数
localparam MIN_SPEED = FCLK / MIN_FS;  // 最小速度，对应最大分频系数
localparam INIT_SPEED =FCLK / INIT_FS ; // 启动速度，默认为最小速度


localparam ENABLE = 1'b0;
localparam DISABLE= 1'b1;

localparam MAX_DECEL_STEP = (MIN_SPEED - MAX_SPEED ) * ADJUST_STEPS / DECEL + EXT_STEPS; // 最大速度减速到最低速度所需要的脉冲数
localparam MAX_ACCEL_STEP = (MIN_SPEED - INIT_SPEED ) / ACCEL ;// 起步加速到最大速度的脉冲数

/*------------------- 参数检查 -------------------- */
initial begin
    if(MAX_FS < MIN_FS) $error("MAX_FS < MIN_FS");
    if(INIT_FS < 0 ) $error("INIT_FS < 0 ");
    if(ACCEL <0) $error("ACCEL<0") ;
    if(DECEL <0) $error("DECEL <0") ;
    if(FCLK < MAX_FS) $error("FCLK < MAX_FS");
    if(ADJUST_STEPS<1) $error("ADJUST_STEPS<1") ;
end

`ifdef DEBUG
initial begin 
    $display("MAX_SPEED=%d",MAX_SPEED);
    $display("MIN_SPEED=%d",MIN_SPEED);
    $display("INIT_SPEED=%d",INIT_SPEED);
end 
`endif 

/*---------------------- reg ---------------------- */
reg [W-2:0] walkStep_r ;
reg [0  :0] walkDir_r ;
reg [1:0] st ;
reg [W-1:0] clkcnt ;
reg signed [W-1:0]step_d1 ;
reg signed [W-1:0]step_d2 ;
reg [1:0] start_d ;
reg signed [SW-1:0] x_r ;  
reg [1:0]zero_d ;
reg autoStop ;
reg [W-1:0] current_divider; // 当前分频系数
reg [W-1:0] steps_to_decel; // 开始减速的步数
reg [W-1:0] adj_cnt ; 
/*----------------------- wire ---------------------*/
wire ena ; 
wire [W-2:0] walkStep_w ;
/*--------------------- assign ---------------------*/
assign ena = ~|clkcnt;
assign o_busy = st==IDLE ? 1'b0:1'b1 ;
assign o_x = x_r ;
assign walkStep_w = step_d1[W-1] ? ~step_d1[W-2 : 0]+1'b1: step_d1[W-2 : 0] ;
/*---------------------- blk -----------------------*/

always@(posedge clk) begin
    step_d1 <= i_step ; 
    step_d2 <= step_d1 ;
    start_d <= {start_d[0] , i_start} ;
    zero_d <= {zero_d[0] , i_zero} ;
end

always@(posedge clk) begin
    if(~|clkcnt | ~rst_n | st==IDLE )begin
        clkcnt <= current_divider ; // i_divider ;
    end else begin
        clkcnt <= clkcnt -1 'b1 ;
    end
end

always @(posedge clk ) begin
    if(!rst_n)begin
        st <= IDLE ;
        o_dir <=1'b0;
        o_en  <=DISABLE ; 
        o_pulse <= 1'b0;
        x_r <= $signed(0);
        walkDir_r <= 1'b0;
        walkStep_r <= {(W-1){1'b0}} ;
        autoStop  <= 1'b0 ;
    end else begin
        case(st)
            IDLE:   begin
                o_pulse <= 1'b0 ;
                autoStop <=1'b0 ;
                if(start_d == 2'b01) begin
                    walkStep_r <= walkStep_w ;
                    walkDir_r  <= step_d1[W-1] ;
                    steps_to_decel <= MAX_DECEL_STEP ;
                    st <= SYNC;
                end
            end
            SYNC: begin
                if(~|walkStep_r) begin
                    st <= IDLE ;
                end
                else if(ena)begin
                    o_en  <= ENABLE ;
                    o_dir <= walkDir_r ;
                    o_pulse <= 1'b1;
                    walkStep_r <= walkStep_r - 1'b1;
                    st <=  HIGH ;
                end
            end
            HIGH:begin
                if(ena) begin
                    st <=  LOW ;
                    o_pulse <= 1'b0;
                end
            end
            LOW:begin
                if(ena) begin
                    o_pulse <= 1'b0 ;
                    if(~|walkStep_r)begin
                        st <= IDLE;
                        o_en <= DISABLE ;
                    end else begin
                        walkStep_r <= walkStep_r - 1'b1 ;
                        st <= HIGH ; 
                        o_pulse <= 1'b1;
                    end
                    x_r <= walkDir_r==1'b1 ? x_r - 1'b1 : x_r + 1'b1 ;
                    if(autoStop) begin
                        st <= IDLE ;
                        o_en <= DISABLE;
                        o_pulse <= 1'b0;
                    end
                end
            end 
            default:begin
                st <= IDLE ;
            end
        endcase
        
        if(zero_d == 2'b01) begin
            x_r <= $signed(0) ;
            if(st !=IDLE) begin
                autoStop <= 1'b1;
            end
        end
    end
end

// 加减速控制 

// always@(posedge clk) begin 
//     if(!rst_n) begin
//         current_divider <= INIT_SPEED ;    
//         adj_cnt <= {W{1'b0}} ;
//     end else begin
//         if(EN_ACC)begin 
//             case(st)
//                 IDLE:begin
//                     if(start_d == 2'b01) begin
//                         current_divider <= INIT_SPEED ;
//                         adj_cnt <= ADJUST_STEPS - 1'B1;
//                     end
//                 end
//                 LOW:begin
//                     if(ena)begin
//                         if(~|adj_cnt)begin
//                             if(walkStep_r < steps_to_decel ) begin
//                                 current_divider <= SpeedDown(current_divider) ;
//                             end else begin
//                                 current_divider <= SpeedUp(current_divider) ;
//                             end
//                             adj_cnt <= ADJUST_STEPS - 1'B1;
//                         end else begin
//                             adj_cnt <= adj_cnt - 1'b1;
//                         end
//                     end
//                 end
//             endcase 
//         end else begin
//             current_divider <= i_divider ; 
//         end
//     end
// end 

generate
    if(EN_ACC==1)begin:acc1
        always@(posedge clk) begin 
            if(!rst_n) begin
                current_divider <= INIT_SPEED ;    
                adj_cnt <= {W{1'b0}} ;
            end else begin
                case(st)
                    IDLE:begin
                        if(start_d == 2'b01) begin
                            current_divider <= INIT_SPEED ;
                            adj_cnt <= ADJUST_STEPS - 1'B1;
                        end
                    end
                    LOW:begin
                        if(ena)begin
                            if(~|adj_cnt)begin
                                if(walkStep_r < steps_to_decel ) begin
                                    current_divider <= SpeedDown(current_divider) ;
                                end else begin
                                    current_divider <= SpeedUp(current_divider) ;
                                end
                                adj_cnt <= ADJUST_STEPS - 1'B1;
                            end else begin
                                adj_cnt <= adj_cnt - 1'b1;
                            end
                        end
                    end
                endcase 
            end
        end 
    end else begin:uniform_speed
        always@(posedge clk) current_divider <= i_divider ;
    end
endgenerate


endmodule


`endif
